Multiple Target Tracking with Navigation Uncertainty

The goal of concurrent mapping and localization (CML) is for a mobile robot to build a map of an unknown environment while simultaneously using that map to navigate. CML can be considered as a problem of multiple target tracking (MTT) in the presence of navigation uncertainty. Although data association errors can have a catastrophic e ect on CML performance, previous approaches to CML, such as stochastic mapping (SM), have either ignored the data association problem, matched features by hand, or used a nearest-neighbor approach [4, 2]. We have developed Integrated Mapping and Navigation (IMAN), a multiple hypothesis approach to CML that generalizes SM to incorporate data association uncertainty and expands multiple hypothesis tracking (MHT) to accommodate navigation error. This paper summarizes IMAN and illustrates its performance for a simulation of an autonomous underwater vehicle (AUV) navigating with forward-looking sonar. SM uses a monolithic state vector and error covariance matrix. In IMAN, the vehicle model and each proposed feature are represented by a distinct model. This model separation is introduced to provide increased exibility in modeling as well as to account for the multiple simultaneous estimates which are allowed within a multiple hypothesis framework. At any given time step, IMAN produces a set of state estimates and estimate error covariances for the vehicle and each feature. These estimates represent the possibilities depending on which of the proposed hypotheses are ultimately accepted as valid. Although the combination of a multiple-hypothesis approach and vehicle navigational uncertainty prevents the clustering or partitioning common to multiple target tracking, state estimate separability is maximized using decision dependency sets. In this way, orthogonal decisions do not result in increased track tree size. A new technique for assignment formation and likelihood calculation integrates target-to-track and data-to-data associations. New track updating procedures account for the inseparability of target-totrack and data-to-data associations. Modi ed pruning strategies reduce algorithmic complexity and ensure track tree consistency. Figure 1 summarizes the IMAN algorithm, which consists of the following steps [3]: 1. state projection: possible vehicle and feature states from the previous cycle are projected using the individual vehicle and feature models; 2. match hypothesis formation: measurements are compared to existing feature models to determine possible data association matches; 3. additional hypothesis formation: additional hypotheses concerning measurement source, existing feature disposition, and new feature identi cation are formed in cases where data association is ambiguous; 4. feature track updating: feature track trees are updated to re ect the feature disposition hypotheses which have been formed; 5. vehicle track updating: the vehicle track tree is updated to re ect the possible navigational events for the current cycle; and 6. pruning: the vehicle and feature track trees are pruned to remove estimates re ecting unlikely events and to enforce consistency.